/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SmartDashboard;
import edu.wpi.first.wpilibj.can.CANTimeoutException;

/**
 *
 * @author Administrator
 */
public class ArmPercentVBus {

    private Joystick m_armJoy;
    private CANJaguar m_shoulder;
    private CANJaguar m_elbow;
    private DigitalInput m_shoulderNegativeLimit;
    private DigitalInput m_elbowNegativeLimit;
    private DigitalInput m_shoulderPositiveLimit;
    private DigitalInput m_elbowPositiveLimit;
    private static final double kMinShoulderPercentV = -1; // Should be negative
    private static final double kMaxShoulderPercentV = 1; // Should be positive
    private static final double kMinElbowPercentV = -1; // Should be negative
    private static final double kMaxElbowPercentV = 1; // Should be positive
    private DriverStation m_ds = DriverStation.getInstance();

    public ArmPercentVBus(int shoulderCANDevice, int elbowCANDevice, Joystick dualAnalogJoystick, int shoulderNegativeLimitChannel, int shoulderPositiveLimitChannel, int elbowNegativeLimitChannel, int elbowPositiveLimitChannel) {
        try {
            m_armJoy = dualAnalogJoystick;
            m_shoulder = new CANJaguar(shoulderCANDevice, CANJaguar.ControlMode.kPercentVbus);
            m_shoulderNegativeLimit = new DigitalInput(shoulderNegativeLimitChannel);
            m_shoulderPositiveLimit = new DigitalInput(shoulderPositiveLimitChannel);
            m_elbow = new CANJaguar(elbowCANDevice, CANJaguar.ControlMode.kPercentVbus);
            m_elbowNegativeLimit = new DigitalInput(elbowNegativeLimitChannel);
            m_elbowPositiveLimit = new DigitalInput(elbowPositiveLimitChannel);
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println(e.getMessage());
            e.printStackTrace();
        }
    }

    private double getShoulderPercentVBus() {
        double percentJoy = m_armJoy.getY();
        double percentVBus = 0;
        if (percentJoy > 1) {
            percentJoy = 1;
        } else if (percentJoy < -1) {
            percentJoy = -1;
        }
        if (m_shoulderNegativeLimit.get() || m_shoulderPositiveLimit.get()) {
            percentJoy = 0;
        }

        if (percentJoy > 0) {
            percentVBus = kMaxShoulderPercentV * percentJoy;
        }

        if (percentJoy < 0) {
            // Joystick value is a negative percentage of the minimum.
            percentVBus = kMinShoulderPercentV * -percentJoy;
        }

        return percentVBus;
    }

    private double getElbowPercentVBus() {
        double percentJoy = m_armJoy.getRawAxis(5);
        double percentVBus = 0;
        if (percentJoy > 1) {
            percentJoy = 1;
        } else if (percentJoy < -1) {
            percentJoy = -1;
        }

        if (m_elbowNegativeLimit.get() || m_elbowPositiveLimit.get()) {
            percentJoy = 0;
        }
        if (percentJoy > 0) {
            percentVBus = kMaxElbowPercentV * percentJoy;
        }

        if (percentJoy < 0) {
            // Joystick value is a negative percentage of the minimum.
            percentVBus = kMinElbowPercentV * -percentJoy;
        }
        return percentVBus;
    }

    public void update() {
        try {
            m_shoulder.setX(getShoulderPercentVBus());
            m_elbow.setX(getElbowPercentVBus());
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println(e.getMessage());
            e.printStackTrace();
        }
    }

    public void setAutonomousArmPercentVBus(double shoulderPercentVBus, double elbowPercentVBus) {
        if (m_ds.isAutonomous()) {
            try {
                m_shoulder.setX(shoulderPercentVBus);
                m_elbow.setX(elbowPercentVBus);
            } catch (CANTimeoutException e) {
                DriverStationPrinter.getInstance().println(e.getMessage());
                e.printStackTrace();
            }
        }
    }

    /**
     * Updates the information on the SmartDashboard.
     */
    public void updateSmartDashboard() {
        try {
            SmartDashboard.log(m_shoulder.getX(), "Shoulder Percent VBus");
            SmartDashboard.log(m_elbow.getX(), "Elbow Percent VBus");
        } catch (CANTimeoutException e) {
            DriverStationPrinter.getInstance().println(e.getMessage());
            e.printStackTrace();
        }
    }
}
